# !/usr/bin/env python
# -*- coding: UTF-8 -*-
import copy
import math
import pyproj


# ecef转化为经纬高
def lla2ecef(x, y, z):
    ecef = pyproj.Proj(proj='geocent', ellps='WGS84', datum='WGS84')
    lla = pyproj.Proj(proj='latlong', ellps='WGS84', datum='WGS84')
    x, y, alt = pyproj.transform(lla, ecef, x, y, z, radians=False)  # radians否用弧度返回值
    return x, y, alt


def transform(lon, lat, z):
    p = pyproj.Proj("epsg:32650")
    x, y = p(lon, lat)
    return x, y, z


def proj_trans(x, y, z):
    p1 = pyproj.Proj(init="epsg:4326")  # 定义数据地理坐标系
    p2 = pyproj.Proj(init="epsg:32650")  # 定义转换投影坐标系
    x1, y1 = p1(x, y)
    x2, y2 = pyproj.transform(p1, p2, x1, y1, radians=True)
    return x2, y2, z


def distance(p1, p2):
    return math.sqrt(math.pow(p1[0] - p2[0], 2) + math.pow(p1[1] - p2[1], 2) + math.pow(p1[2] - p2[2], 2))


class VelocityDetector(object):
    def __init__(self):
        self.lastpoint = None
        self.last_v = 0.0
        self.routes = []
        self.rate = 1

    def __del__(self):
        self.routes = []

    def push_back(self, gps):
        self.routes.append(gps)

    def calculate_velocity(self):
        if len(self.routes) is 0:
            return False, 0.0

        buf = copy.deepcopy(self.routes)
        self.routes = []

        if self.lastpoint is not None:
            buf.insert(0, self.lastpoint)

        dist = 0.0
        i = 0
        while i < len(buf) - 1:
            p1 = transform(buf[i].lontitude, buf[i].latitude, buf[i].altitude)
            p2 = transform(buf[i + 1].lontitude, buf[i + 1].latitude, buf[i + 1].altitude)
            dist += distance(p1, p2)
            i += 1
        self.lastpoint = buf[len(buf) - 1]

        if len(buf) - 1 is not 0:
            velocity = dist / (len(buf) - 1)
            # print("velocity%f" % (velocity * 3600 / 1000))
            return True, (velocity * 3600 / 1000)
        else:
            return False, 0.00
